00100	IFN USER+TRACK,<0>
00200	STRADR:	MOVE BITSAV	;Enters here every 1/60 th. seccond
00300	IFE USER+TRACK,<
00400		TRNE DTST
00500		CALLI 400024
00600		DATAO ARM,[2]
00700		SOSLE STWT
00800		CALLI 400024
00900	>
01000		MOVE P,[XWD -PDLL,PDL]	;PUSH DOWN STACK
01100		MOVE CBUF,CBUFS
01200		MOVE DATWD,DATWDS
01300		IORI DATWD,2		;YELLOW ARM
01400	IFN REPORT+TRACK,<
01500		MOVE DATA,DATPT
01600	>
01700	IFN USER+TRACK,<
01800		TRNE DTST
01900		JRST SWDONE
02000	>
02100	IFE USER,<
02200	IFN TRACK,<
02300		SKIPE WALK
02400		JRST NCHK
02500	>
02600		CONSZ 40		;CHECK TO SEE IF ON PDP-6
02700		JRST[	MOVEI TAC,PDPERR
02800			MOVEM TAC,TRAJER
02900			JRST TOFF]
03000	;	CONO 4,400		;TURN OFF THE PI'S
03100		CONI 0,AC		;SET PROCESSOR FLAGS
03200		ANDI AC,7
03300		CONO 0,2050(AC)
03400		TRZE TRIP		;TURN ON ARM
03500		DATAO ARM,[3]
03600	>
03700	NCHK:
03800		MOVEI K,51B23		;-10 VOLT REFERENCE
03900	IFE USER,<PUSHJ P,PREAD		;REFTR← -10 volt reference
04000		LDB AC,SNUM		;PICK UP MIDDLE SAMPLE
04100		ANDI DACVAL,7777	;LEAVE LAST SAMPLE IN J
04200		ADDI AC,(DACVAL)	;ADD THEM TOGETHER
04300	>
04400	IFN USER,<MOVEI AC,17770>
04500		CAIGE AC,17000		;IS THE FUSE BLOWN
04600		JRST[		MOVEI TAC,REFERR
04700				MOVEM TAC,TRAJER
04800				JRST TOFF]
04900		MOVEM AC,REFTR		;REFERENCE READING
05000	IFN TRACK,<
05100		SKIPE WALK
05200		JRST TOK
05300	>
05400	IFE USER,<
05500		CONI CLOCK,AC
05600		TLZ AC,777774
05700		JRST STIME		;TIME IN MICRO SECONDS
05800	>
05900	IFN USER,<JRST .+2>
     

00100	NXTJIF:
00200	IFN USER,<JRST SWDONE>
00300	IFN DEB<
00400		MOVEM 17,ACSAV+17
00500		MOVEI 17,ACSAV
00600		BLT 17,ACSAV+16
00700		MOVE 17,ACSAV+17
00800	>
00900	IFN TRACK,<
01000		SKIPE WALK
01100		JRST TOK
01200	>
01300	IFN REPORT+TRACK,<
01400		HRRZI TAC,(DATA)
01500		CAIL TAC,DATO+DBL-1
01600		JRST[	SUB DATA,[XWD DBL,DBL]
01700			MOVE TAC,[XWD DATO+DBL,DATO]
01800			BLT TAC,(DATA)
01900			HRRZI AC,(DATA)
02000			HRRZ TAC,DATIWD
02100			SUBI AC,(TAC)
02200			JRST RDP]
02300		HRRZI AC,(DATA)
02400		HRRZ TAC,DATIWD
02500		SUBI AC,(TAC)
02600		HRRZ MQ,DATPT
02700		SUBI TAC,(MQ)
02800		IMUL AC,TAC
02900	RDP:	SKIPLE AC
03000	IFN TRACK,<PUSH DATA,[0]>
03100	IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
03200		MOVEI AC,DATO-1
03300		HRRZ TAC,DATIWD
03400	 	CAIL TAC,DATO+DBL-1
03500		HRRM AC,DATIWD
03600		MOVEM DATA,DATPT
03700	>
03800	IFE USER,<
03900		MOVEI TAC,=2000
     

00100	WATSTL:	CONI CLOCK,AC
00200		TLZ AC,777774
00300		SUB AC,LTIME
00400		JUMPGE AC,.+2
00500		ADD AC,[=1000000]
00600	IFN REPORT,<
00700		CAIN TAC,=2000
00800		JRST[	PUSH DATA,[<SIXBIT/TICK/>+1]
00900			HRRZ MQ,DATIWD
01000			PUSH DATA,MQ
01100			JRST .+1]
01200	>
01300		MOVEM AC,ETIME
01400		TRNE RUN
01500		JRST[	MOVE MQ,AC
01600			FSC MQ,233
01700			FMPR MQ,[0.00006]
01800			MOVEM MQ,TDF
01900			MOVE AC,LTIME
02000			ADD AC,ETIME
02100			JRST SSTIME]
02200		CAIGE AC,=16667		;delta T greater than 16.7 msec.?
02300		SOJGE TAC,WATSTL
02400		JUMPL TAC,TE
02500		CAIL AC,=19000
02600		JRST[	MOVEI TAC,ISERR
02700			MOVEM TAC,TRAJER
02800			JRST TOFF]
02900		MOVSI AC,(1.0)
03000		MOVEM AC,TDF
03100		MOVE AC,LTIME
03200		ADDI AC,=16667
03300	SSTIME:	TLNE AC,777774
03400		SUB AC,[=1000000]
03500	STIME:	MOVEM AC,LTIME		;LTIME←LTIME+16.7 msec.
03600	>
03700	IFN USER,<
03800		MOVEI AC,=8888
03900		MOVEM AC,ETIME
04000		MOVSI AC,(0.5)
04100		MOVEM AC,TDF
04200	>
04300		SOSGE COUNT
04400		JRST[TE:MOVEI AC,TERR
04500			MOVEM AC,TRAJER
04600			JRST ZITO]
04700	IFN ADJUST,<
04800		MOVEI K,14B23
04900		MOVEM BITSAV
05000		MOVEI I,2
05100	RNP:	PUSHJ P,PREAD
05200		LDB AC,SNUM
05300		FSC AC,221
05400		JSA 16,EXP
05500		JUMP AC
05600		FMPR [0.15]
05700		MOVEM FACTOR(I)
05800		SOJGE I,RNP
05900		MOVE BITSAV
06000		MOVE I,ADJI
06100		JUMPL I,MAIN
06200	IFN ADJG,<
06300		MOVE AC,ADJE
06400		FMPR AC,FACTOR
06500		MOVEM AC,KE(I)		;POT 12
06600		MOVE AC,ADJV
06700		FMPR AC,FACTOR+1
06800		MOVEM AC,KV(I)		;POT 13
06900		MOVE AC,ADJIF
07000		FMPR AC,FACTOR+2
07100		MOVEM AC,KI(I)	;POT 14
07200	>
07300	IFN ADJD,<
07400		MOVE AC,ADJE
07500		FMPR AC,FACTOR
07600		MOVEM AC,KM(I)		;POT 12
07700		MOVE AC,ADJV
07800		FMPR AC,FACTOR+1
07900		MOVEM AC,PK(I)		;POT 13
08000	>
08100	>
     

00100	MAIN:	TRNE RUN
00200		JRST[	PUSHJ P,EVAL
00300			PUSHJ P,NXTH
00400			JRST .+1]
00500		MOVEI J,6
00600		PUSHJ P,ANGLES		;read joint angles
00700		TRNE RONLY		;read angles only?
00800		JRST[	PUSHJ P,HEAD	;read hand also
00900			MOVE AC,HAND
01000			MOVEM AC,SET
01100			PUSHJ P,REFLEX
01200			PUSHJ P,READED]	;DOES NOT RETURN
01300		TRNN RUN
01400		JRST[	SKIPGE TAC,FUNCT	;not run, mabye a function
01500			JRST[	PUSHJ P,SETSET
01600				JRST NEXT]
01700			JUMPE TAC,[CAIL CBUF,BUF;try next buffer
01800				SETOM (CBUF)	;dont do this if first buffer
01900				MOVEM CBUF,CBUFS
02000				ADDI CBUF,1001
02100				CAILE CBUF,BUF2
02200				MOVEI CBUF,BUF		;Advance buffer
02300				MOVE 2,(CBUF)
02400				JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
02500					JRST SWDONE]
02600				JUMPN 2,BUFRDY		;the last buffer stop.
02700				JRST[	TRO DTST
02800					MOVEI 2,WAITIN
02900					HRRM 2,JUMPBK
03000					MOVEM 2,STKPTR
03100			IFN REPORT,<PUSH DATA,[0]>
03200					JRST BUFWT]
03300			BUFRDY:	HRRZ 2,(CBUF)
03400				ADDI 2,-1(CBUF)
03500				HRRZM 2,STKPTR
03600				JRST NEXT]
03700			CAIN TAC,6
03800			JRST SETCH
03900			JRST TT]	;otherwise perform function
04000		JUMPE J,[RUDONE:TRZ DROP+RUN+FINAL+WOB	 	;if J is zero then end of run
04100			TDZ DATWD,[770001252500]
04200			PUSHJ P,SETSET
04300			TRZE STP
04400			JRST[	MOVEI TAC,STPERR
04500				MOVEM TAC,TRAJER
04600				POP P,TAC
04700				JRST TOFF]
04800			JRST NEXT]
     

00100	TT:	PUSHJ P,REFLEX
00200	DISP:	MOVE AC,FUNCT
00300		CAILE AC,MAXFN
00400		JRST NEXT
00500		JRST @.+1(AC)		;Perform function
00600		NXTJIF	;0
00700		HOPEN	;1
00800		HCLOSE	;2
00900		NXTJIF	;3
01000		PLACE	;4
01100		NUDGE	;5
01200		SETCH	;6
01300		STOP	;7
01400		SAVE	;10
01500		RESTORE	;11
01600		CENTER	;12
01700		SET.ARM	;13 SET_ARM
01800		WOBBLE	;14
01900		SEARCH	;15
02000		AOJI	;16
02100		SLAVE	;17
02200	
02300		XLIST
02400		SUBTTL	SPACEWAR CODE	ANGLE MEASUREMENT	LOU PAUL
02500		LIST
     

00100	ANGLES:	MOVEI I,5
00200		JFCL 10,.+1
00300		MOVE TAC,[XWD TH,THP]
00400		BLT TAC,THP+5
00500		MOVEI L,10
00600	IFN REPORT,<PUSH DATA,[<SIXBIT/THETA/>+6]>
00700	SETCHA:	MOVEI K,CHA
00800		TRNE ALT6
00900		MOVEI K,N6CHA
01000		SKIPA
01100	RSTRT:	MOVE K,CHAN(I)
01200		SOJL L,REDER
01300	IFE USER,<
01400		PUSHJ P,PREAD
01500		LDB AC,SNUM
01600		CAIN I,5
01700		JRST[	TRNE POTREAD
01800			JRST .+1
01900			CAIL AC,1000
02000			CAILE AC,7000
02100			JRST[	TRC ALT6
02200				JRST SETCHA]
02300			MOVEI K,CHA5
02400			JRST .+1]
02500		ANDI DACVAL,7777
02600		ADDI AC,(DACVAL)
02700		SETZ TAC,
02800		DIV AC,REFTR
02900		ASH AC,-26
03000		MOVE TAC1,AC
03100		TRNE POTREAD
03200		JRST[	ADDI TAC1,=1000
03300			JRST EXON]
03400		ASHC AC,-11		;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
03500		LDB MQ,NON(I)		;GET LOWER DIFF
03600		ADDI TAC1,(MQ)		;ADD IT TO READING
03700		AOJ AC,
03800		LDB AC,NON(I)		;GET UPPER DIFFERENCE
03900		SUBI AC,(MQ)
04000		MUL TAC,AC		;INTERPOLATE
04100		ADD TAC1,TAC		;CORRECTED READING
04200		FSC TAC1,226		;FLOAT
04300		FMPR TAC1,SCALE(I)
04400		FADR TAC1,OFF(I)
04500	>
04600	IFN USER,<
04700		MOVE TAC1,ERR(I)
04800		FSC TAC1,-1
04900		FADR TAC1,T0(I)
05000	>
05100		CAIN I,5
05200		JRST[	TRNE ALT6
05300			FADR TAC1,OFF6
05400			FADR TAC1,ROT6
05500			MOVEM TAC1,TH+5
05600			FSBR TAC1,T0+5
05700			MOVSI MQ,(180.0)
05800			CAML TAC1,MQ
05900			JRST[	INCROT:	MOVN MQ,MQ
06000					FSC MQ,1
06100					FADRM MQ,ROT6
06200					FADRM MQ,TH+5
06300					JRST THS]
06400			MOVN MQ,MQ
06500			CAMG TAC1,MQ
06600			JRST INCROT
06700			JRST THS]
06800	EXON:	MOVEM TAC1,TH(I)		;THETA
06900	THS:	CAILE I,2
07000		JRST[	MOVE AC,TH(I)
07100			FSBR AC,THP(I)
07200			FDVR AC,TDF
07300			MOVEM AC,TD(I)
07400			JRST SERVO]
07500	IFN REPORT,<
07600		MOVE MQ,TH(I)
07700		FSBR MQ,T0(I)
07800		PUSH DATA,MQ
07900	>
     

00100	IFE USER,<
00200		MOVE K,VCHAN(I)
00300	NVEL:	PUSHJ P,PREAD
00400		LDB AC,SNUM
00500		ANDI DACVAL,7777
00600		ADDI AC,(DACVAL)
00700		TRNE VZERO
00800		MOVEM AC,VZ(I)
00900		SUB AC,VZ(I)
01000		FSC AC,216
01100		FMPR AC,VSCALE(I)
01200	IFN TACH,<
01300		TRNE VZERO
01400		SETZM VDTH(I)
01500		FADRM AC,VDTH(I)
01600	>
01700		MOVEM AC,TD(I)
01800	>
01900		XLIST
02000				SUBTTL			SPACEWAR CODE			SERVO CALC.			LOU PAUL
02100		LIST
     

00100	SERVO:	TRNN RUN
00140		JRST[	SOJGE I,RSTRT
00170			POPJ P,]
00200	SERVL:	MOVE TAC,TH(I)
00300		FSBR TAC,T0(I)		;POSITION ERROR
00400		MOVEM TAC,ET0(I)
00500		TRNE FINAL
00600		JRST NULL		;MODIFY THETA BY INTEGRAL TERM
00700		MOVE TAC1,T0(I)
00800		FSBR TAC1,T0P(I)
00900		FDVR TAC1,TDF
01000		MOVE AC,TAC1
01100		FSBR AC,TDP(I)
01200		MOVEM TAC1,TDP(I)
01300		FDVR AC,TDF
01400		MOVEM AC,TDD(I)
01500	MODT0:	MOVE MQ,CII(I)
01600		FMPR AC,MQ
01700		HRRZ K,BMASK(I)
01800		TDNN K,FBI
01900		JRST [	PDIR:SETZM ET0(I)
02000			JRST NOBACK]
02100		FMPR TAC,KI(I)
02200		FMPR MQ,MQ
02300		FDVR TAC,MQ
02400		FADRM TAC,ERRINT(I)
02500	IFE STEP,<
02600		MOVN TAC,TD(I)
02700		FADR TAC,TAC1
02800	>
02900	IFN STEP,<MOVN TAC,TD(I)>
03000		FMPR TAC,KV(I)		;VELOCITY GAIN
03100		FADR AC,TAC
03200		MOVN TAC,ET0(I)
03300		FMPR TAC,KE(I)		;ERROR GAIN
03400		FDVR TAC,CII(I)
03500		FADR AC,TAC
03600	
03700		FADR AC,ERRINT(I)
03800	
03900	NOBACK:	CAIE I,2
04000		FMPR AC,[IRAD:0.0174532925]	;JOINT 3 IS IN INCHES
04100		MOVEM AC,TORE(I)
04200		FADR AC,CI(I)		;GRAVITY TERM
04300		MOVEM AC,T(I)			;ERROR CORRECTION TORQUE
04400		XLIST
04500				SUBTTL			SPACEWAR CODE			DRIVE CALC.			LOU PAUL
04600		LIST
     

00100	DRIVE:
00200	IFN REPORT,<PUSH DATA,[<SIXBIT/DAC/>+6]>
00300	LS0:	TDNN DATWD,BMASK(I)
00400		JRST ISP
00500		MOVE MQ,TD(I)
00600		JUMPN MQ,.+2
00700		MOVN MQ,ET0(I)
00800		FMPR MQ,T(I)
00900		CAIN I,1
01000		JRST[	HRRZ AC,FBI
01100			TDNN AC,BMASK(I)
01200			JRST .+1
01300			TRNE DROP
01400			JRST .+1
01500			SETZ MQ,
01600			JRST NOEX]
01700		JUMPL MQ,[	MOVM MQ,T(I)
01800				FSBR MQ,F0(I)
01900				JUMPL MQ,[	FDVR MQ,F0(I)
02000						FMPR MQ,V0(I)
02100						FSC MQ,1
02200						FADR MQ,V0(I)
02300						SKIPGE T(I)
02400						MOVN MQ,MQ
02500						JRST CEMF]
02600				MOVSI AC,(1.0)
02700				FSBR AC,PK(I)
02800				FMPR MQ,AC
02900				FMPR MQ,KM(I)
03000				FADR MQ,V0(I)
03100				SKIPG T(I)
03200				MOVN MQ,MQ
03300				JRST CEMF]
03400		SKIPN T(I)
03500		JRST[		SKIPN TD(I)
03600				JRST CEMF
03700				MOVE MQ,V0(I)
03800				SKIPGE TD(I)
03900				MOVN MQ,MQ
04000				JRST CEMF]
04100		JUMPE MQ,NOEX
04200		MOVE MQ,V0(I)
04300		SKIPGE T(I)
04400		MOVN MQ,MQ
04500	NOEX:	MOVSI AC,(1.0)
04600		FADR AC,PK(I)
04700		FMPR AC,T(I)
04800		FMPR AC,KM(I)
04900		FADR MQ,AC
05000	CEMF:
05100		MOVE AC,EMF(I)
05200		FMPR AC,TD(I)
05300		MOVSI TAC1,(30.0)
05400		JUMPGE MQ,DRVLT
05500		MOVN AC,AC
05600	DRVLT:	FSBR TAC1,AC			;Available drive voltage
05700		CAMGE TAC1,[1.0]
05800		JRST MAXDRV
05900	VELOK:	MOVM AC,MQ
06000		FDVR AC,TAC1		;AC= REL TIME ON
06100		FIX AC,211000		;1=16
06200		CAML AC,[4000000]
06300		JRST DHO		;too much force stop the arm
06400		CAILE AC,776000
06500	MAXDRV:	MOVEI AC,776000
06600	IFN REPORT+TRACK+USER+DEB,<
06700		MOVEM AC,MOTOR(I)
06800		SKIPGE MQ
06900		MOVNS MOTOR(I)
07000	>
07100		TRC AC,400000
07200		HRL AC,I
07300	SETDRV:
07400	IFN TRACK,<SKIPN WALK>
07500		DATAO WIDTH,AC
07600		TDZ DATWD,DMASK(I)
07700		JUMPGE MQ,ISP
07800		TDO DATWD,DMASK(I)
07900	ISP:	SKIPE REV(I)
08000		TDC DATWD,DMASK(I)
08100	IFN REPORT,<PUSH DATA,MOTOR(I)>
08200		DATAO ARM,DATWD
08300		SOJGE I,RSTRT
08400		JFCL 10,DHO
08500	IFN TRACK,<SKIPN WALK>
     

00100		TRZ VZERO
00200		SETZM ASTP
00300		TRNE STP
00400		JRST[	MOVEI I,5
00500		MTL:	MOVE MQ,TORE(I)
00600			FDVR MQ,F0(I)
00700			SKIPN TD(I)
00800			FSC MQ,-2
00900			FMPR MQ,STQ(I)
01000			FADRM MQ,ASTP
01100			SOJGE I,MTL
01200			MOVE AC,ASTP
01300			CAML AC,SDTQ
01400			JRST[	AOS AC,ASTPP
01500				CAIL AC,3
01600				JRST[	TRZ STP
01700					SUB P,[XWD 1,1]
01800					SETZM ERRINT
01900					MOVE AC,[XWD ERRINT, ERRINT+1]
02000					BLT AC,ERRINT+5
02100					JRST RUDONE]
02200				POPJ P,]
02300			SETZM ASTPP
02400			POPJ P,]
02500		POPJ P,
02600	
02700	NULL:	SETZB AC,TAC1
02800		TLNN DATWD,NNUL
02900		TRNE WOB
03000		SOJA J,MODT0
03100		TDNN DATWD,GMASK(I)
03200		SOJA J,DRIVE
03300		MOVE MQ,FBI
03400		TDNN MQ,GMASK(I)
03500		SOJA J,PDIR
03600		MOVM MQ,TAC			;|POSITION ERROR|
03700		CAMLE MQ,ERR(I)			;COMPARE POSITION ERROR
03800		JRST MODT0
03900		MOVM MQ,TD(I)
04000		CAMLE MQ,ERR(I)
04100		JRST MODT0
04200	SO:	TDZ DATWD,BMASK(I)
04300	IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
04400		DATAO ARM,DATWD
04500		HRLZ MQ,I
04600		TRC MQ,400000
04700		DATAO WIDTH,MQ
04800		SOJA J,DRIVE
04900		POPJ P,
05000	
05100		XLIST
05200		SUBTTL	SPACEWAR CODE				LOU PAUL
05300		LIST
     

00100	NXTH:	MOVE AC,[XWD T0,T0P]
00200		BLT AC,T0P+5
00300		TRNE WOB
00400		JRST[	MOVEI I,2
00500		WL1:	SOSGE J,WOBCNT(I)
00600			JRST[	MOVEI J,=19
00700				MOVEM J,WOBCNT(I)
00800				JRST WL2]
00900		WL2:	MOVE AC,SIN(J)
01000			FMPR AC,WOBMAG
01100			FADRM AC,TFF+3(I)
01200			SOJGE I,WL1
01300			JRST .+1]
01400		TRNE DROP
01500		JRST[	MOVEI I,5
01600		OL:	MOVE AC,DELTH(I)
01700			FMPR AC,TDF
01800			FADRM AC,DTH(I)
01900			SOJGE I,OL
02000			JRST JALS]
02100	NTH:
02200		TRNE INCREM
02300		JRST[	MOVE MQ,ETIME
02400			ADDB MQ,TICKS
02500			CAML MQ,NTICKS
02600			JRST[	TRZ INCREM
02700				SKIPN TP
02800				TRO FINAL
02900				MOVEI AC,=600
03000				MOVEM AC,COUNT
03100				MOVEI I,5
03200			FIL:	MOVE AC,DELTH(I)
03300				FADRB AC,DTH(I)
03400				FADR AC,TFF(I)
03500				MOVEM AC,T0(I)
03600				SETZM DELTH(I)
03700				SOJGE I,FIL
03800				POPJ P,]
03900			SETZ I,
04000			DIV MQ,NTICKS
04100			ASH MQ,-10
04200			FSC MQ,200
04300			MOVSI AC,(6.0)
04400			FMPR AC,MQ
04500			FADR AC,[-15.0]
04600			FMPR AC,MQ
04700			FADR AC,[10.0]
04800			FMPR AC,MQ
04900			FMPR MQ,MQ
05000			FMPR MQ,AC
05100			MOVEI I,5
05200		LDC:	MOVE AC,DELTH(I)
05300			FMPR AC,MQ
05400			FADR AC,DTH(I)
05500			FADR AC,TFF(I)
05600			MOVEM AC,T0(I)
05700			SOJGE I,LDC
05800			POPJ P,]
05900	JALS:	MOVEI I,5
06000	JAL:	MOVE AC,DTH(I)
06100		FADR AC,TFF(I)
06200		MOVEM AC,T0(I)
06300		SOJGE I,JAL
06400		POPJ P,
06500	
     

00100	EVAL:	MOVEI I,5
00200	EL:	MOVE J,TP(I)	;POINTS TO KOE5
00300		JUMPE J,NE
00400		MOVE MQ,ETIME
00500		ADDB MQ,TJ(I)
00600		CAML MQ,TN(I)	;NUMBER OF TICKS THIS SEGMENT
00700		JRST[			;END OF SEGMENT
00800			CAIN I,5
00900			JRST[			;JOINT 6 HAS CONTROL WORD
01000				MOVE TAC,[XWD NC,PC]
01100				BLT TAC,PC+=11	;UPDATE PC
01200				MOVE TAC,TICKS
01300				CAMGE TAC,NTICKS
01400				TRO INCREM
01500				LDB TAC,[POINT 3,1(J),2]	;LEVEL
01600				JUMPN TAC,[			;SWITCH POINT
01700					SKIPG LOOP(TAC)
01800					JRST[			;INITIALIZE LOOP
01900						LDB AC,[POINT 15,1(J),17]
02000						MOVEM AC,LOOP(TAC)	;COUNT
02100						JRST SETALT]
02200					SOSLE LOOP(TAC)		;IN LOOP
02300				SETALT:	TRO ALT
02400					JRST GETNXT]
02500				JRST GETNXT]
02600		GETNXT:	TRNE ALT
02700			JRST[	LDB J,[POINT 9,(J),8]	;ALT POINTER
02800				JRST GOT]
02900			LDB J,[POINT 9,(J),17]		;NEXT POINTER
03000			JUMPE J,[SETZM TP(I)
03100				TRO FINAL
03200				MOVEI AC,=600
03300				MOVEM AC,COUNT
03400				MOVE TAC,[XWD NC,CI]
03500				BLT TAC,CI+=11
03600				MOVE AC,A4(I)
03700				ADD AC,A3(I)
03800				ADD AC,A2(I)
03900				ADD AC,A1(I)
04000				ADD AC,A0(I)
04100				XOR AC,SC(I)
04200				FSC AC,0
04300				MOVEM AC,TFF(I)
04400				JRST NE]
04500		GOT:	ADDI J,(CBUF)
04600			MOVEM J,TP(I)
04700			SUB MQ,TN(I)	;MICRO SEC INTO NEW SEGMENT
04800			MOVEM MQ,TJ(I)	;INITIALIZE
04900			HRRZ AC,(J)
05000			ASH AC,16
05100			MOVEM AC,TN(I)
05200			CAIN I,5
05300			JRST[
05400				HRRZ TAC,1(J)
05500				ADDI TAC,(CBUF)
05600				MOVEI MQ,14(TAC)
05700				HRLI TAC,(TAC)
05800				HRRI TAC,NC
05900				BLT TAC,NC+=11
06000				MOVEI K,5
06100			AAI:	MOVE TAC,MOTARM(K)
06200				FADRM TAC,NC+6(K)
06300				SOJGE K,AAI
06400				MOVEI K,5
06500				MOVE MQ,(MQ)
06600			FTF:	MOVE TAC,GMASK(K)
06700				TDNN TAC,FBI
06800				JRST[	TDNN TAC,MQ
06900					JRST FNF
07000					MOVE AC,TH(K)
07100					MOVEM AC,T0(K)
07200					FSBR AC,TFF(K)
07300					MOVEM AC,DTH(K)
07400					MOVNM AC,DELTH(K)
07500					MOVE AC,TN+5
07600					ASH AC,16
07700					MOVEM AC,NTICKS
07800					SETZM TICKS
07900					TRO INCREM
08000					JRST FNF]
08100				TDNN TAC,MQ
08200				JRST[	MOVE AC,TDD(K)
08300					CAIE K,2
08400					FMPR AC,IRAD
08500					FMPR AC,CII(K)
08600					FADR AC,T(K)
08700					MOVEM AC,CI(K)
08800					JRST FNF]
08900			FNF:	SOJGE K,FTF
09000				HRRZM MQ,FBI
09100				MOVEI K,=11
09200			DELC:	MOVE AC,NC(K)
09300				FSBR AC,PC(K)
09400				MOVEM AC,DCI(K)
09500				SOJGE K,DELC
09600				JRST DDELC]
09700		DDELC:	HLRE AC,-3(J)
09800			MOVEM AC,A4(I)
09900			HRRE AC,-3(J)
10000			MOVEM AC,A3(I)
10100			HLRE AC,-2(J)
10200			MOVEM AC,A2(I)
10300			HRRE AC,-2(J)
10400			MOVEM AC,A1(I)
10500			HLLZ AC,-1(J)
10600			MOVEM AC,SC(I)
10700			HRRE AC,-1(J)
10800			MOVEM AC,A0(I)
10900			MOVE MQ,TJ(I)
11000			JRST ELL]
11100	ELL:	DIV MQ,TN(I)
11200		CAIN I,5
11300		JRST[	MOVEI K,=11
11400			HLRE TAC,MQ
11500			FSC TAC,211
11600		DECC:	MOVE AC,DCI(K)
11700			FMPR AC,TAC
11800			FADR AC,PC(K)
11900			MOVEM AC,CI(K)
12000			SOJGE K,DECC
12100			JRST .+1]
12200		MOVE AC,A4(I)
12300		MUL AC,MQ
12400		ADD AC,A3(I)
12500		MUL AC,MQ
12600		ADD AC,A2(I)
12700		MUL AC,MQ
12800		ADD AC,A1(I)
12900		MUL AC,MQ
13000		ADD AC,A0(I)
13100		XOR AC,SC(I)
13200		FSC AC,0
13300		MOVEM AC,TFF(I)
13400		CAIN I,5
13500		JRST[	MOVE AC,TFF+5
13600			FSBR AC,T0+5
13700			MOVSI MQ,(180.0)
13800			CAML AC,MQ
13900			JRST[	INCT0:	FSC MQ,1
14000					FADRM MQ,T0+5
14100					JRST .+1]
14200			MOVN MQ,MQ
14300			CAMG AC,MQ
14400			JRST INCT0
14500			JRST .+1]
14600	NE:	SOJGE I,EL
14700		TRZ ALT
14800		POPJ P,
14900	
     

00100	DHO:	AOJ I,			;DISASTER HAS OCCURED
00200		LSH I,3			;JOINT NUMBER
00300		IORI I,EXER
00400		MOVEM I,TRAJER
00500		POP P,I
00600	ZITO:	SETZM ERRINT
00700		MOVE AC,[XWD ERRINT,ERRINT+1]
00800		BLT AC,ERRINT+5
00900	
01000	TOFF:	MOVE AC,STKPTR
01100		MOVEM AC,ERRSTK
01200		MOVEI L,1000
01300		TRNE RONLY
01400		JRST READED
01500		TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB		;STOP THE ARM
01600		SETZM TP
01700		MOVE AC,[XWD TP,TP+1]
01800		BLT AC,TP+5
01900		PUSHJ P,SETSET
02000	MOVNXT:	AOS AC,STKPTR
02100	MOVNX:	SOJL L,[ LE:
02200			MOVEI TAC,LOOPER
02300			MOVEM TAC,TRAJER
02400			JRST PW]
02500		HLRE TAC,@STKPTR
02600		CAIN TAC,3		;IS IT A WAIT
02700		JRST MOVNXT
02800		CAIN TAC,SKPERR
02900		JRST[	HRRZ TAC,(AC)
03000			CAMN TAC,TRAJER
03100			JRST[	AOS STKPTR
03200				JRST NEXT]
03300			JRST MOVNXT]
03400		CAIN TAC,SKPNER
03500		JRST[	HRRZ TAC,(AC)
03600			CAME TAC,TRAJER
03700			JRST[	AOS STKPTR
03800				JRST MOVNXT]
03900			JRST NEXT]
04000		CAIN TAC,SKPSER
04100		JRST[	LDB MQ,[POINT 3,(AC),35]
04200			LDB TAC,[POINT 3,TRAJER,35]
04300			CAIN TAC,(MQ)
04400			JRST[	HRRZ TAC,(AC)
04500				ANDI TAC,777770
04600				TDNE TAC,TRAJER
04700				JRST[	AOS STKPTR
04800					JRST NEXT]
04900				JRST MOVNXT]
05000			JRST MOVNXT]
05100		CAIN TAC,JMPREL
05200		JRST[	HRRE TAC,(AC)
05300			ADDI AC,(TAC)
05400			HRRM AC,STKPTR
05500			JRST MOVNX]
05600	PW:	CAIL AC,BUF
05700		HRRM AC,JUMPBK
05800		MOVEI AC,WAITIN
05900		MOVEM AC,STKPTR
06000		MOVEI AC,3
06100		MOVEM AC,FUNCT
06200	IFN TRACK,<SKIPN WALK>
06300		DATAO ARM,[2]		;STOP THE ARM
06400	IFN REPORT,<PUSH DATA,[0]>
06500	READED:	TRO DTST
06600	IFN DEB,<
06700		MOVEM 17,ACSAV+17
06800		MOVEI 17,ACSAV
06900		BLT 17,ACSAV+16
07000	>
07100		JRST SWDONE
07200	
     

00100	NEXT:	TRNE RUN
00200		JRST[	SETZM FUNCT
00300			JRST NXTJIF]
00400		MOVEI L,1000
00500		MOVE AC,FUNCT
00600		CAIE AC,6		;SET TOUCH
00700		JRST[	MOVE TAC,SOTCHS
00800			MOVEM TAC,SOTCH
00900			TRZ STCH
01000			JUMPE TAC,.+1
01100			TRO STCH
01200			JRST .+1]
01300	IFN TRACK,<SKIPN WALK>
01400		MOVEI DATWD,2
01500		DATAO ARM,DATWD		;STOP THE ARM
01600	INCPIC:	AOS AC,STKPTR
01700	PICKUP:	SOJL L,LE
01800		HLRE TAC,@STKPTR
01900		CAIN TAC,JMPABS
02000		JRST[JMPA:HRRZ TAC,@STKPTR
02100			MOVEM TAC,STKPTR
02200			HLRZ TAC,@STKPTR
02300			CAIN TAC,3
02400			JRST INCPIC
02500			JRST PICKUP]
02600		CAIN TAC,SKPERR
02700		JRST[	HRRZ TAC,@STKPTR
02800			CAMN TAC,TRAJER
02900			AOS STKPTR
03000			JRST INCPIC]
03100		CAIN TAC,SKPNER
03200		JRST[	HRRZ TAC,@STKPTR
03300			CAME TAC,TRAJER
03400			AOS STKPTR
03500			JRST INCPIC]
03600		CAIN TAC,SKPSER
03700		JRST[	LDB MQ,[POINT 3,@STKPTR,35]
03800			LDB TAC,[POINT 3,TRAJER,35]
03900			CAIN TAC,(MQ)
04000			JRST[	HRRZ TAC,@STKPTR
04100				ANDI TAC,777770
04200				TDNE TAC,TRAJER
04300				AOS STKPTR
04400				JRST INCPIC]
04500			JRST INCPIC]
04600		CAIN TAC,JMPREL
04700		JRST[	HRRE TAC,@STKPTR
04800			ADD TAC,STKPTR
04900			HRRM TAC,STKPTR
05000			JRST PICKUP]
05100		SETZM TRAJER
05200		MOVEI I,100
05300		MOVEM I,COUNT
05400		JUMPL TAC,[TRO RUN+VZERO
05500			SETCA TAC,
05600			MOVEM TAC,FUNCT
05700			AOS DOING		;INCREASE TRAJECTORY NUMBER
05800			HLRZ 2,(CBUF)
05900			ASH 2,16
06000			MOVEM 2,NTICKS		;RUN TIME
06100			MOVEI 2,=1800
06200			MOVEM 2,COUNT
06300			SETZM LOOP
06400			MOVE TAC,[XWD LOOP,LOOP+1]
06500			BLT TAC,LOOP+9
06600			MOVE DATWD,777(CBUF)
06700			HRRZM DATWD,FBI
06800			MOVEI I,5
06900			MOVEI K,34
07000		SUL:	SUBI K,4
07100			HRLZM K,TJ(I)
07200			HRLZM K,TN(I)
07300			MOVEI TAC,TJ(I)
07400			MOVEM TAC,TP(I)
07500			TDNE DATWD,BMASK(I)
07600			TDO DATWD,BMASK(I)
07700			SETZM DTH(I)
07800			SOJGE I,SUL
07900	
08000			HRLZI TAC,763(CBUF)
08100			HRRI TAC,NC
08200			BLT TAC,NC+=11
08300			MOVEI I,5
08400		SAAI:	MOVE TAC,MOTARM(I)
08500			FADRM TAC,NC+6(I)
08600			SOJGE I,SAAI
08700			SETZM ETIME
08800			PUSHJ P,EVAL
08900			TRZ INCREM
09000			MOVEI I,5
09100		MOVBK:	MOVE AC,TH(I)
09200			MOVEM AC,T0(I)
09300			FSBR AC,TFF(I)
09400			CAIN I,5
09500			JRST[	MOVSI MQ,(180.0)
09600				CAML AC,MQ
09700				JRST[SETROT:FSC MQ,1
09800					MOVN MQ,MQ
09900					FADRM MQ,TH+5
10000					FADRM MQ,ROT6
10100					JRST MOVBK]
10200				MOVN MQ,MQ
10300				CAMG AC,MQ
10400				JRST SETROT
10500				JRST ADDTF]
10600		ADDTF:	MOVEM AC,DTH(I)
10700			MOVN AC,AC
10800			FADRM AC,DELTH(I)
10900			SETZM TD(I)
11000			SOJGE I,MOVBK
11100			SETZM TICKS
11200			HLRE TAC,@STKPTR
11300			JRST MARK]
11400		MOVEM TAC,FUNCT
11500		CAIN TAC,3		;IS IT A WAIT
11600		JRST[	MOVE TAC,STKPTR
11700			CAIL TAC,BUF
11800			HRRM TAC,JUMPBK
11900			MOVEI TAC,WAITIN
12000			MOVEM TAC,STKPTR
12100	IFN REPORT,<PUSH DATA,[0]>
12200			TRO DTST
12300			PUSHJ P,SWDONE]
12400	MARK:
12500	IFN REPORT,<
12600		PUSH DATA,[<SIXBIT/NEXT/>+1]
12700		MOVE TAC,FUNCT
12800		HRL TAC,0
12900		PUSH DATA,TAC
13000	>
13100	
13200	;	JRST DISP
13300	SWDONE:
13400		MOVEM BITSAV
13500		MOVEM DATWD,DATWDS
13600		MOVEM CBUF,CBUFS
13700	IFN REPORT+TRACK,<
13800		MOVEM DATA,DATPT
13900	>
14000	IFE USER,<
14100	IFN TRACK,<
14200		SKIPE WALK
14300		JRST @STRADR-1
14400	>
14500	;	CONO 4,200		;TURN ON THE PI'S
14600		CALLI 400024
14700	>
14800	IFN USER,<
14900		JRST @STRADR-1
15000	>
15100	
15200	SETSET:	MOVEI I,5
15300	TCHK:	MOVE J,TP(I)
15400		JUMPE J,ATEND
15500		SETZM TJ(I)
15600		SETZM TN(I)
15700	GNN:	LDB J,[POINT 9,(J),17]
15800		ADDI J,(CBUF)
15900		CAIN J,(CBUF)
16000		JRST[	MOVEM J,TP(I)
16100			JRST ATEND]
16200		JRST GNN
16300	ATEND:	SOJGE I,TCHK
16400		PUSHJ P,EVAL
16500		MOVEI I,5
16600	FDTH:	MOVE AC,TH(I)
16700		MOVEM AC,T0(I)
16800		FSBR AC,TFF(I)
16900		MOVEM AC,DTH(I)
17000		SETZM DELTH(I)
17100		SETZM TDP(I)
17200		SETZM TDD(I)
17300		SOJGE I,FDTH
17400		POPJ P,
17500	
     

00100	PREAD:
00200	IFN TRACK,<
00300		SKIPE WALK
00400		JRST[GNPR:AOBJP DATA,[	MOVE AC,[IOWD DBL,DATO]
00500					MOVEM AC,DATPT
00600					MOVE DATA,DATPT
00700					SETZ TAC,
00800					INPUT 15,1
00900					JRST GNPR]
01000			MOVE DACVAL,(DATA)
01100			JRST @READFN]
01200	>
01300		MOVEI TAC,10
01400		MOVEM TAC,DATMSD
01500	DACRED:	SOSGE DATMSD
01600		JRST[REDER:	MOVEI TAC,READER	
01700				MOVEM TAC,TRAJER
01800				JRST TOFF]
01900	DACST:	CONO DB,4250
02000		CONO AD,(K)
02100	NREAD:	MOVEI TAC,30
02200		CONSO DB,1000
02300		SOJGE TAC,.-1
02400		JUMPL TAC,[DACDIE:CONO AD,4000(K)	;STOP DAC
02500				TRO REREAD
02600			IFN DEB,<MOVEM DACVAL,DACERR>
02700				MOVEI TAC,12
02800			WRH:	SOJGE TAC,WRH
02900				JRST DACRED]
03000		DATAI DB,DACVAL
03100		CONSZ DB,10000
03200		JRST DACDIE
03300		XOR DACVAL,SBMSK
03400	IFN TRACK,<PUSH DATA,DACVAL>
03500		JRST @READFN
03600	FREAD:	ADDI K,770000
03700	IFN TRACK,<SKIPN WALK>
03800		CONO AD,4000(K)	;STOP AD ON NEXT CHANNEL
03900		POPJ P,
04000	READFN:	FREAD
04100	
04200		XLIST
04300		SUBTTL	CONSTANTS				LOU PAUL
04400		LIST
     

00100	NON:	POINT 12,INNER(AC),11
00200		POINT 12,INNER(AC),23
00300		POINT 12,INNER(AC),35
00400		POINT 12,OUTER(AC),11
00500		POINT 12,OUTER(AC),23
00600		POINT 12,OUTER(AC),35
00700	ERR:	0.05
00800		0.05
00900		0.01
01000		0.1
01100		0.1
01200		0.7
01300	TH:	BLOCK 6		;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
01400	HAND:	3.0
01500	TDD:	BLOCK 6
01600	ERRINT:	0
01700		0
01800		0
01900		0
02000		0
02100		0
02200	DTH:	BLOCK 6
02300	TD:	BLOCK 6
02400	T0P:	BLOCK 6
02500	TDP:	BLOCK 6
02600	ET0:	BLOCK 6
02700	T0:	-150.0
02800		-75.0
02900		14.0
03000		-90.0
03100		 60.0
03200		 0.0000
03300	TFF:	-150.0
03400		-75.0
03500		14.0
03600		-90.0
03700		 60.0
03800		 0.0000
03900	DELTH:	BLOCK 6		;THIS AND THE NEXT ARE BLT ED TOGETHER
04000	NTICKS:	0
04100	DTHS:	BLOCK =30
04200	TORE:	BLOCK 6
04300	T:	BLOCK 6
04400	IFN USER+REPORT+TRACK+DEB,<MOTOR:	BLOCK 6>
04500	FBI:	0
04600	VZ:	BLOCK 3
04700	IFN TACH,<
04800	VDTH:	BLOCK 3
04900	>
05000	THP:	BLOCK 6
05100	MOTARM:	700000.0
05200		800000.0
05300		5000.0
05400		3000.0
05500		3000.0
05600		4000.0
05700	STQ:	BLOCK 6
05800	SDTQ:	0
05900	ASTP:	0
06000	ASTPP:	0
06100	PC:	BLOCK 14
06200	DCI:	BLOCK 6
06300	DCII:	BLOCK 6
06400	CI:	0
06500		0
06600		0
06700		0
06800		0
06900		0
07000	CII:	500000.0
07100		700000.0
07200		 10000.0
07300		100000.0
07400		100000.0
07500		  1000.0
07600	NC:	BLOCK =12
07700	IFN DEB,<DACERR:	0>
07800	REFTR:	0
07900	ETIME:	=8888
08000	TDF:	0.5
08100	IFN DEB,<
08200	ETIM:	0
08300	ACSAV:	BLOCK 20
08400	>
08500	PDL:	DHO
08600		BLOCK PDLL
08700	LTIME:	0
08800	FUNCT:	0
08900	ROT6:	0
09000	TRAJER:	0
09100	DOING:	0
09200	DATMSD:	0
09300	CBUFS:	0
09400	DATWDS:	0
09500	SNUM:	POINT 12,DACVAL,23
09600	SBMSK:	400040004000
09700	REV:	0
09800		0
09900		-1
10000		0
10100		-1
10200		-1
10300	DMASK:	400000
10400		100000
10500		20000
10600		4000
10700		1000
10800		200
10900	BMASK:	XWD 400000,200000
11000		XWD 200000, 40000
11100		XWD 100000, 10000
11200		XWD  40000,  2000
11300		XWD  20000,   400
11400		XWD  10000,   100
11500	GMASK:	200000
11600		 40000
11700		 10000
11800		  2000
11900		   400
12000		   100
12100	VCHAN:	52B23
12200		53B23
12300		54B23
12400	CHAN:	55B23
12500		56B23
12600		57B23
12700		60B23
12800		61B23
12900	GAIN:	MOVEI I,5
13000	SETGN:	MOVE AC,KV(I)
13100		FMPR AC,AC
13200		FSC AC,-2
13300		MOVEM AC,KE(I)
13400		FMPR AC,KV(I)
13500		FSC AC,-4
13600		MOVNM AC,KI(I)
13700		SOJGE I,SETGN
13800		MOVE 1,[SIXBIT/$ARM$/]
13900		CALLI 1,400002
14000		MOVE I,RET
14100		MOVEM I,GAIN
14200		JRST GAIN
14300	RET:	POPJ P,
14400		BLOCK 11
14500		0
14600	WAITIN:	XWD 3,0
14700	JUMPBK:	XWD JMPABS,WAITIN
14800	STKPTR:	WAITIN
14900	ERRSTK:	0
15000	BITSAV:	0
15100	COUNT:	0
15200	STWT:	0
15300	HCNT:	0
15400	TP:	0
15500		0
15600		0
15700		0
15800		0
15900		0
16000	TJ:	BLOCK 6
16100	LOOP:	BLOCK 10
16200	TN:	BLOCK 6
16300	SC:	BLOCK 6
16400	A0:	BLOCK 6
16500	A1:	BLOCK 6
16600	A2:	BLOCK 6
16700	A3:	BLOCK 6
16800	A4:	BLOCK 6
16900	BLOCKS:	0
17000	NXTBUF:	0
17100	BUF:	0
17200		BLOCK 1000
17300	BUF2:	0
17400		BLOCK 1000
17500	IFN ADJUST<
17600	ADJI:	-1
17700	ADJE:	0
17800	ADJV:	0
17900	ADJIF:	0
18000	FACTOR:	BLOCK 3
18100	>
18200	IFN REPORT+TRACK,<
18300	DATIWD:	0
18400		0
18500	DATPT:	0
18600	DATO:	BLOCK DBL+100
18700	>
18800	IFN DYNAMICS,<
18900	DDAT:	<SIXBIT/FORD/>+6
19000	FOR:	BLOCK 6
19100		<SIXBIT/BACK/>+6
19200	BAK:	BLOCK 6
19300	>